#include "graph_optimizer/vertex/vertex_bg.hpp"

namespace g2o
{
    VertexBg::VertexBg() : BaseVertex<3, Eigen::Vector3d>() { setToOriginImpl(); }
    bool VertexBg::read(std::istream &is)
    {
        Eigen::Vector3d lv;
        for (int i = 0; i < 3; i++)
            is >> lv[i];
        setEstimate(lv);
        return true;
    }

    bool VertexBg::write(std::ostream &os) const
    {
        Eigen::Vector3d lv = estimate();
        for (int i = 0; i < 3; i++)
        {
            os << lv[i] << " ";
        }
        return os.good();
    }
} // namespace g2o